

from cv2 import exp
import rclpy
from rclpy.node import Node
from carsource_interfaces.msg import GlobalPathPlanning
from carsource_interfaces.msg import Goal
from geometry_msgs.msg import Pose

import yaml

from PIL import Image
import cv2
import numpy as np

import math
import matplotlib.pyplot as plt

show_animation = False


class GlobalPathNode(Node):
    """
        sub : 
            topic: goal 
            type : int64 x_pos int64 y_pos

            param goal

        pub : 
            topic : globalpath 
            type : int64[] x_pos int64[] y_pos

            param rx ry
    """
    def __init__(self, name,map_path):
        super().__init__(name)
        # 初始化
        self.resolution = 0 # 地图 分辨率
        
        self.robot_radius = 0.05 # 机器人半径
        self.min_x = 0 # 障碍物最靠近x轴距离
        self.min_y = 0 # 障碍物最靠近y轴距离
        self.max_x = 0 # 障碍物离x轴最远距离
        self.max_y = 0 # 障碍物离y轴最远距离
        self.x_width = 0 #膨胀地图x轴的宽度
        self.y_width = 0 #膨胀地图y轴的宽度
        self.obstacle_map = None #膨胀地图
        self.ox = [] #全局地图的障碍物x轴坐标
        self.oy = [] #全局地图的障碍物y轴坐标
        self.origin = []

        
        yaml_path = map_path + "map.yaml"
        pgm_path = map_path + "map.pgm"

        self.resolution, self.origin = self.yaml_read(yaml_path)
        
        self.img_ = self.read_img(pgm_path)

        #从 self.img_ 中加载障碍物点
        self.get_obstacle()
        #降低存储量
        self.calc_obstacle_map()

        print("全局路径的障碍物加载成功")  
        
        #subscriber
        self.subcurrent_goal = self.create_subscription(Goal,"goal",self.recv_goal,1)
        # self.subcurrent_pose = self.create_subscription(Pose,"current_pose", self.rev_cur,1)
        #publisher
        self.pub_global_path = self.create_publisher(GlobalPathPlanning,"global_path_planning", 1) 

    #NOTE debug
    def rev_cur(self, msg:Pose):
        plt.ion()
        x = msg.position.x
        y = msg.position.y

        plt.plot(x, y, ".y")
        plt.pause(0.01)
        # plt.show()

    def quart_to_yaw(self, orientation):
            # roll = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
            # pitch = math.asin(2 * (w * y - x * z))
            yaw = math.atan2(2 * (orientation[0] * orientation[3] + orientation[1] * orientation[2]),
                                1 - 2 * (orientation[3] * orientation[3] + orientation[2] * orientation[2]))
    
            return yaw

    def yaml_read(self, yaml_path):
        f = open(yaml_path,'r',encoding='utf-8')
        cont = f.read()
        f_info = yaml.load(cont)
        
        resolution = f_info['resolution']
        
        origin = f_info['origin']

        return resolution,origin

    def read_img(self,pgm_name):
        img_ = cv2.imread(pgm_name,0)
        # cv2.namedWindow('img_', cv2.WINDOW_FREERATIO)   # 窗口大小自适应比例
        # cv2.imshow("img_", img_)
        
        
        return img_

    def get_obstacle(self):

        '''

                |
                |
                |
        height  |
                |
                |||||||||||||||||||
                   width 

        shape 第一个返回的是 height 第二个是 width
        '''
        height, width = self.img_.shape
        # cv2.imshow("omg",self.img_)
        
        for i in range(height):
            for j in range(0,width):
                if(self.img_[i,j] == 0):
                    self.ox.append(j)
                    self.oy.append(i)

        
        # print(height,width)

        # cv2.waitKey(0)
    def calc_obstacle_map(self):

        self.min_x = round(min(self.ox))
        self.min_y = round(min(self.oy))
        self.max_x = round(max(self.ox))
        self.max_y = round(max(self.oy))


        self.x_width = round((self.max_x - self.min_x) )
        self.y_width = round((self.max_y - self.min_y) )


        # obstacle map generation
        self.obstacle_map = [[False for _ in range(self.y_width)]
                            for _ in range(self.x_width)]
        for ix in range(self.x_width):
            for iy in range(self.y_width):
                for iox, ioy in zip(self.ox, self.oy):
                    d = math.hypot((iox - self.min_x - ix), (ioy - self.min_y - iy)) * self.resolution
                    if d <= self.robot_radius:
                        self.obstacle_map[ix][iy] = True
                        # plt.plot(ix,iy,'.k')
                        break
        
        # plt.show()
    
    def recv_goal(self, goal):
        print("receive goal message : ", goal.x_goal, goal.y_goal)

        #TODO 硬编码 后续需要替换
        sx = 238
        sy = 242
        # plt.ion()    # 打开交互模式

        # if 1:  # pragma: no cover
        #     plt.plot(self.ox, self.oy, ".k")
        #     plt.plot(sx, sy, "og")
        #     plt.plot(goal.x_goal, goal.y_goal, "xb")
        #     plt.grid(True)
        #     plt.axis("equal")


        # ax = plt.gca()                                 #获取到当前坐标轴信息
        # ax.xaxis.set_ticks_position('top')   #将X坐标轴移到上面
        # ax.invert_yaxis()  
                                  #反转Y坐标轴

        dijkstra = self.Dijkstra(self.resolution,self.robot_radius,
                                 self.min_x,self.min_y, self.max_x,self.max_y,
                                 self.x_width,self.y_width,
                                 self.obstacle_map)  

        #NOTE 接受终点坐标
        rx, ry = dijkstra.planning(sx,sy, goal.x_goal, goal.y_goal)    

        pub_msg = GlobalPathPlanning()
        print(len(rx))
        rx.reverse()
        ry.reverse()
        pub_msg.x_pos = rx
        pub_msg.y_pos = ry
        
        print("pub global path")
        self.pub_global_path.publish(pub_msg)
        # if 1:  # pragma: no cover
        #         plt.plot(rx, ry, "-r")
                # plt.pause(0.01)
          #      plt.show()
        # plt.show()

    class Dijkstra:
        def __init__(self, resolution,robot_radius,
                            min_x,min_y, max_x,max_y,
                            x_width,y_width,
                            obstacle_map):
            """
            Initialize map for a star planning

            ox: x position list of Obstacles [m]
            oy: y position list of Obstacles [m]
            resolution: grid resolution [m]
            rr: robot radius[m]
            """

            self.min_x = min_x
            self.min_y = min_y
            self.max_x = max_x
            self.max_y = max_y
            self.x_width = x_width
            self.y_width = y_width
            self.obstacle_map = obstacle_map

            self.resolution = resolution
            self.robot_radius = robot_radius
           
            self.motion = self.get_motion_model()

        class Node:
            def __init__(self, x, y, cost, parent_index):
                self.x = x  # index of grid
                self.y = y  # index of grid
                self.cost = cost
                self.parent_index = parent_index  # index of previous Node

            def __str__(self):
                return str(self.x) + "," + str(self.y) + "," + str(
                    self.cost) + "," + str(self.parent_index)

        def planning(self, sx, sy, gx, gy):
            """
            dijkstra path search

            input:
                s_x: start x position [m]
                s_y: start y position [m]
                gx: goal x position [m]
                gx: goal x position [m]

            output:
                rx: x position list of the final path
                ry: y position list of the final path
            """
            # 聚焦到obastacle map 中
            start_node = self.Node(self.calc_xy_index(sx, self.min_x),
                                self.calc_xy_index(sy, self.min_y), 0.0, -1)

            print("start_node.x : ",start_node.x)
            print("start_node.y : ",start_node.y)

            goal_node = self.Node(self.calc_xy_index(gx, self.min_x),
                                self.calc_xy_index(gy, self.min_y), 0.0, -1)

            print("goal_node.x : ",goal_node.x)
            print("goal_node.y : ",goal_node.y)

            open_set, closed_set = dict(), dict()
            open_set[self.calc_index(start_node)] = start_node

            while 1:
                try:
                    c_id = min(open_set, key=lambda o: open_set[o].cost)
                    current = open_set[c_id]
                except ValueError as e:
                    print("无法进行轨迹规划，🙏重新规划选择目标终点")
                    break
                # show graph
                # if show_animation:  # pragma: no cover
                #     plt.plot(self.calc_position(current.x, self.min_x),
                #             self.calc_position(current.y, self.min_y), "xc")
                #     # for stopping simulation with the esc key.
                #     plt.gcf().canvas.mpl_connect(
                #         'key_release_event',
                #         lambda event: [exit(0) if event.key == 'escape' else None])
                #     if len(closed_set.keys()) % 10 == 0:
                #         plt.pause(0.001)

                if current.x == goal_node.x and current.y == goal_node.y:
                    print("Find goal")
                    goal_node.parent_index = current.parent_index
                    goal_node.cost = current.cost
                    break

                # Remove the item from the open set
                del open_set[c_id]

                # Add it to the closed set
                closed_set[c_id] = current

                # expand search grid based on motion model
                for move_x, move_y, move_cost in self.motion:
                    node = self.Node(current.x + move_x,
                                    current.y + move_y,
                                    current.cost + move_cost, c_id)
                    n_id = self.calc_index(node)

                    if n_id in closed_set:
                        continue

                    if not self.verify_node(node):
                        continue

                    if n_id not in open_set:
                        open_set[n_id] = node  # Discover a new node
                    else:
                        if open_set[n_id].cost >= node.cost:
                            # This path is the best until now. record it!
                            open_set[n_id] = node

            rx, ry = self.calc_final_path(goal_node, closed_set)

            return rx, ry

        def calc_final_path(self, goal_node, closed_set):
            # generate final course
            # 最后需要聚焦到之前pgm的坐标
            rx, ry = [goal_node.x + self.min_x], [goal_node.y + self.min_y]
            parent_index = goal_node.parent_index
            while parent_index != -1:
                n = closed_set[parent_index]
                rx.append(n.x + self.min_x)
                ry.append(n.y + self.min_y)
                parent_index = n.parent_index

            return rx, ry

        def calc_position(self, index, minp):
            pos = index  + minp
            return pos
            
        def calc_xy_index(self, position, minp):
            return round(position - minp)

        def calc_index(self, node):
            return node.y  * self.x_width + node.x 

        def verify_node(self, node):
            px = self.calc_position(node.x, self.min_x)
            py = self.calc_position(node.y, self.min_y)

            if px < self.min_x:
                return False
            if py < self.min_y:
                return False
            if px >= self.max_x:
                return False
            if py >= self.max_y:
                return False

            if self.obstacle_map[node.x][node.y]:
                return False

            return True

        

        @staticmethod
        def get_motion_model():
            # dx, dy, cost
            motion = [[1, 0, 1],
                    [0, 1, 1],
                    [-1, 0, 1],
                    [0, -1, 1],
                    [-1, -1, math.sqrt(2)],
                    [-1, 1, math.sqrt(2)],
                    [1, -1, math.sqrt(2)],
                    [1, 1, math.sqrt(2)]]

            return motion




def main(args=None):

    rclpy.init(args=args)


    node = GlobalPathNode("global_path","/home/maotian/Ros2project/ros2_ws/car_ws/src/car_source/map/")

    rclpy.spin(node)

    rclpy.shutdown()
    

    
if __name__ == '__main__':
    main()
